/************************************************

     Brian Bousquet-Smith     Chaz Godwin
                
            OBDII Reader
                 Fall 2011
************************************************/

#include <avr/io.h>
#include <avr/iom164.h> 
#include <avr/interrupt.h>
#include <avr/delay.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>


/************************************************
             Defines
************************************************/

// States


#define OK         0
#define ELM_INIT_STATE    1
#define LCD_INIT_STATE    2
#define    OBD_INIT_STATE    3
#define DASHBOARD    4
#define PERFORMANCE    5
#define DIAGNOSTICS    6
#define ABOUT        7

// ELM Defines

#define COMMAND_END        0x0D

// OBD Defines

#define RESPONSE_END        0x3E



/************************************************
        Function Prototypes
************************************************/

// Initializing Functions

void power_on();
void serial_init();
int elm_init();
int obd_init();
int get_mph();

// Tx Rx Functions

void lcd_transmit(unsigned char data);
void obd_transmit(unsigned char data);
unsigned char lcd_receive();
unsigned char obd_receive();
void lcd_flush();
void obd_flush();
//int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in, char len_out);
//int lcd_sendcmd(unsigned char *in, char len_in);

void print_bits(char byte);


/************************************************
        Global Variables
************************************************/

int state = ELM_INIT_STATE;
int timeoutflag = 0;

/************************************************
        Main Code
************************************************/




int main(void){

/*
*    Purpose: Output RPM and MPH data from a vehicle
*/
    power_on();
    serial_init();
    obd_flush();
    lcd_flush();
    int response;
    _delay_ms(5000);
    while(1){
        PORTA = state;
        switch(state){
        case ELM_INIT_STATE: // Initializes ELM
            lcd_transmit('1');
            response = elm_init();
            if(response == OK){
                state = OBD_INIT_STATE;
                break;
            }
            else{
                PORTA = 0x0F;
                break; 
            }

        case LCD_INIT_STATE: // Initializes LCD
            //response = lcd_init();
            if(response == OK){
                state = OBD_INIT_STATE;
                break;
            }
            else break;
    
        case OBD_INIT_STATE: // Initializes OBDII
            response = obd_init();
            if(response == OK){
                state = DASHBOARD;
                break;
            }
            else{
                PORTA = 0x07; 
                break;
            }

        case DASHBOARD: // Virtual Dashboard
            response = get_mph();
            _delay_ms(50);
            response = get_rpm();
            _delay_ms(50);
            break;
        
        case PERFORMANCE:
            break;        
        
        case DIAGNOSTICS:
            break;
        
        case ABOUT:
            break;
        } 
    }
    return 0;
}

void power_on(){

/*
*    Purpose: Turn the power light on of the device
*    Input: Nothing
*    Output: Nothing
*/

    DDRA=0xFF;
    PORTA = 0x0F;

}

void serial_init(){

/*
*    Purpose: Initialize Serial on Both UART Ports on the AVR
*    Input: Nothing
*    Output: Nothing
*/

    UCSR0A |= (1<<U2X0);            // Double speed for more precision
    UCSR0B |= (1<<RXEN0)|(1<<TXEN0);    // Allow transmit and receive
    UCSR0C |= (0<<USBS0)|(3<<UCSZ00);     // 8 Bit No Parity 1 Stop Bit
    UBRR0H = 0;                // Sets Baud Rate = 9600
    UBRR0L = 12;

    UCSR1A |= (1<<U2X1);            // Double speed for more precision
    UCSR1B |= (1<<RXEN1)|(1<<TXEN1);     // Allow transmit and receive
    UCSR1C |= (1<<USBS1)|(3<<UCSZ10);     // 8 Bit 2 Stop Bits
    UBRR1H = 0;                // Sets Baud Rate = 9600
    UBRR1L = 12;
}

int elm_init(){

/*
*    Purpose: Initialize ELM and check that communication is correct
*    Input: Nothing
*    Output:    Error Codes:
*        0 -    Successful
*        -1 -     Unsuccessful
*/

    unsigned char init_buffer[3] = {'A', 'T', 'Z'};
    int i;
    unsigned char echo;
    unsigned char response;
    unsigned char elm_buffer[12] = {        
        'E', 
        'L', 
        'M', 
        '3', 
        '2', 
        '7', 
        ' ', 
        'v', 
        '1', 
        '.', 
        '4', 
        'b'};
    
    unsigned char elm_response[20];

    // Send ELM the Initialize Command    

    PORTA = 0x07;
    //for(i=0;i<3;i++){
    //    obd_transmit(init_buffer[i]); 
    //    echo = obd_receive();    
    //    if(echo != init_buffer[i]) return -1;
    //}

    obd_flush();
    unsigned char exp_buff[9] = {
    'A', 'T', 'Z', 0x0D, 0x0A, 0x0D, 0x0A, 0x0D, 0x0A};
    obd_transmit('A');
    obd_transmit('T');
    obd_transmit('Z');
    obd_transmit(0x0D);
    obd_receive(); // Gets 'A' Echo
    obd_receive(); // Gets 'T' Echo    
    obd_receive(); // Gets 'Z' Echo
    obd_receive(); // Gets '0x0D' Echo
    obd_receive(); // LED1 Echo
    obd_receive(); // LED2 Echo
    obd_receive(); // LED3 Echo
    obd_receive(); // LED4 Echo
    obd_receive(); // LED5 Echo
    /* At this point OBD should respond with the following
        ELM327 v1.4b
       And lights should flash on the LED connected to the ELM
    */
    int count =0;
    while( (response = obd_receive()) != 0x3E){
        elm_response[count] = response;        
        lcd_transmit(response);
        count++;
    }
    for(i=0;i<12;i++){
        if(elm_response[i] != elm_buffer[i]){
            lcd_transmit(i+42);
            return -1;
        }
    }
    unsigned char test_buffer[10];
    sprintf(test_buffer, "Hello Bri!");
    lcd_sendcmd(test_buffer, sizeof(test_buffer));
    return 0;
    
}
    
int obd_init(){

/*
*    Purpose: Initialize OBDII port and check that communication is correct
*    Input: Nothing
*    Output: Error Codes:
*        0 -    Successful
*        -1 -     Unsuccessful
*/
    
    unsigned char init_buffer[4] = {'0', '1', '0', '0'};
    unsigned char obd_buffer[] = "SEARCHING...";
    int i;
    int flag;
    int count;    
    unsigned char echo;
    unsigned char response;
    
    // Figure out if OBD has valid protocol/is connected
    
    for(i=0;i<4;i++){
        obd_transmit(init_buffer[i]);
        echo = obd_receive();
        if (echo != init_buffer[i]) return -1;
    }
    obd_transmit(COMMAND_END);
    obd_receive(); // Get this echo
    obd_receive(); // Get new line feed     
    
    /* At this point OBD should response with 
        Searching...
           If nothing found outputs
            Unable to Connect
        If found protocol
            41 00 + 4 Bytes of Data 
    */
    flag = 0;
    count = 0;
        
    while( (response = obd_receive()) != RESPONSE_END){
        if(response == 'U') flag = -1;        
        lcd_transmit(response);
    }
    return flag;

}

/*int get_mph (void){

/*
*    Purpose: Get MPH Data and Output It
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

/*    unsigned char mph_buffer[4] = {'0', '1', '0', 'D'};    
    unsigned char exp_data[2] = {0x41, 0x0D};     
    unsigned char response_buffer[3];
    char mph;
    int i, error;
    
    error = obd_sendcmd(mph_buffer, response_buffer, 4, 3);
    if(error != 0) return -1;    
    for(i=0;i<sizeof(exp_data);i++){
        if(response_buffer[i] != exp_data) return -2);
    }
    mph = (unsigned char) response_buffer[2]*.621371;
    unsigned char output_buffer[8];
    sprintf(output_buffer, "MPH: %c", mph);
    int size;
    if(mph >100) size = 8;
    else if(mph < 10) size = 6;
    else size = 7;
    error = lcd_sendcmd(output_buffer, size);
    return error;
}     */


int get_mph(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

    unsigned char rpm_buffer[4] = {'0', '1', '0', 'D'};
    unsigned char rpm_output[9] = {'R', 'P', 'M', ':', 0, 0, 0, 0, 0};
    unsigned char echo;
    unsigned char response;
    int i;
    
    //for(i=0;i<4;i++){
    //    obd_transmit(rpm_buffer[i]);
    //    echo = obd_receive();
    //    if (echo != rpm_buffer[i]) return -1;
    //}
    obd_transmit('0');
    obd_transmit('1');
    obd_transmit('0');
    obd_transmit('D');
    obd_transmit(0x0D);
    
    while( (response = obd_receive()) != RESPONSE_END){
        lcd_transmit(response);
    }
    return 0;
}


int get_rpm(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Error Codes:
*        0 - Successful
*        -1 - Unsuccessful
*/

    unsigned char rpm_buffer[4] = {'0', '1', '0', 'C'};
    unsigned char rpm_output[9] = {'R', 'P', 'M', ':', 0, 0, 0, 0, 0};
    unsigned char echo;
    unsigned char response;
    int i;
    
    //for(i=0;i<4;i++){
    //    obd_transmit(rpm_buffer[i]);
    //    echo = obd_receive();
    //    if (echo != rpm_buffer[i]) return -1;
    //}
    obd_transmit('0');
    obd_transmit('1');
    obd_transmit('0');
    obd_transmit('C');
    obd_transmit(0x0D);
    
    while( (response = obd_receive()) != RESPONSE_END){
        lcd_transmit(response);
    }
    return 0;
}

/*int lcd_sendcmd(unsigned char *in, char len_in){
    
    int i;
    for(i=0;i<len_in;i++){
        lcd_transmit(in);
        ;
    }
    return 0;


}*/
/*    
int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in, char len_out){
    return 0;
}*/




void lcd_transmit (unsigned char data){
    
/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/
    while( !(UCSR1A & (1<<UDRE1)));
    UDR1 = data;
}

unsigned char lcd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/

    while (!(UCSR1A & (1<<RXC1)));
    return UDR1;
}    

void lcd_flush(void){
    char tmp;
    while (UCSR1A & (1<<RXC1) ) tmp = UDR1;
}

void obd_transmit (unsigned char data){

/*
*    Purpose: Transmit data to the LCD
*    Input: Data to be transmitted
*    Output:    Nothing
*/

    while( !(UCSR0A & (1<<UDRE0)));
    UDR0 = data;
}

unsigned char obd_receive (void){

/*
*    Purpose: Receive data from the LCD
*    Input: Nothing
*    Output:    Data received    
*/
    while (!(UCSR0A & (1<<RXC0)));
        
    return UDR0;
}

void obd_flush(void) {
    char tmp;
    while( UCSR0A & (1<<RXC1)) tmp = UDR0;
}

/*char obd_sendcmd (char* in, char* out, char len_in, len_out){    
    //START CLOCK
    int i;    
    for(i=0;i<len_in;i++){
        obd_transmit(in+i);
    }
    for(i=0;i<len_out;i++){


}*/    

//char lcd_sendcmd (char* in, char* out, char len_in, len_out){

//}

void print_bits( char byte){
    char mask[8] = {
    0b10000000,
    0b01000000,
    0b00100000,
    0b00010000,
    0b00001000,
    0b00000100,
    0b00000010,
    0b00000001};

    int m;
    char tmp;
    for(m=0;m<8;m++){
        tmp = ((mask[m]&byte)>>(7-m))+48;
        lcd_transmit(tmp);
    }
}        








    
 